#include <geometry_msgs/msg/twist.hpp>
#include <rclcpp/rclcpp.hpp>
#include <turcle_pose_info/srv/turtle_pose_info.hpp>
#include <turtlesim/msg/pose.hpp>

class TurtlePoseServer : public rclcpp::Node
{
private:
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr
        _volecity_pubulisher;
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr
        _current_pose_subscriper;
    rclcpp::Service<turcle_pose_info::srv::TurtlePoseInfo>::SharedPtr
        _control_server;
    double _turtle_pose_x{5.5};
    double _turtle_pose_y{5.5};

    double _k{1};
    double _maxspeed{3};

    void _control_callback(
        const turcle_pose_info::srv::TurtlePoseInfo::Request::SharedPtr request,
        turcle_pose_info::srv::TurtlePoseInfo::Response::SharedPtr ronsponse)
    {
        if ((request->target_x > 12.0f || request->target_x < 0.0f) ||
            (request->target_y > 12.0f || request->target_y < 0.0f))
        {
            ronsponse->result =
                turcle_pose_info::srv::TurtlePoseInfo::Response::FAIL;
            RCLCPP_INFO(get_logger(), "invalid pose:%f/%f", request->target_x,
                        request->target_y);
        }
        else
        {
            _turtle_pose_x = request->target_x;
            _turtle_pose_y = request->target_y;
            ronsponse->result =
                turcle_pose_info::srv::TurtlePoseInfo::Response::SUCCESS;
            RCLCPP_INFO(get_logger(), "recieve pose:%f/%f", _turtle_pose_x,
                        _turtle_pose_y);
        }
    }

    void _current_callback(const turtlesim::msg::Pose::SharedPtr msg)
    {
        auto con_msg = geometry_msgs::msg::Twist();
        double current_x = msg->x;
        double current_y = msg->y;
        double distance = std::sqrt(std::pow(_turtle_pose_x - current_x, 2) +
                                    std::pow(_turtle_pose_y - current_y, 2));
        double angle =
            std::atan2(_turtle_pose_y - current_y, _turtle_pose_x - current_x) -
            msg->theta;

        if (distance > 0.1)
        {
            // RCLCPP_INFO(get_logger(), "current pose:%f/%f", current_x,
            //             current_y);

            if ((fabs(angle)) > 0.1)
            {
                con_msg.angular.z = _k * abs(angle);
            }
            else
            {
                con_msg.linear.x = std::min(_k * distance, _maxspeed);
            }
            // RCLCPP_INFO(get_logger(),
            //             "volecity set, linear.x: %f, angular.z:%f",
            //             con_msg.linear.x, con_msg.angular.z);
        }
        else
        {
            con_msg.linear.x = 0;
            con_msg.angular.z = 0;
        }
        _volecity_pubulisher->publish(con_msg);
    }

public:
    TurtlePoseServer(const std::string &node_name) : rclcpp::Node(node_name)
    {
        _volecity_pubulisher =
            this->create_publisher<geometry_msgs::msg::Twist>(
                "/turtle1/cmd_vel", 10);

        _control_server =
            this->create_service<turcle_pose_info::srv::TurtlePoseInfo>(
                "/pose_service",
                std::bind(&TurtlePoseServer::_control_callback, this,
                          std::placeholders::_1, std::placeholders::_2));

        _current_pose_subscriper =
            this->create_subscription<turtlesim::msg::Pose>(
                "/turtle1/pose", 10,
                std::bind(&TurtlePoseServer::_current_callback, this,
                          std::placeholders::_1));
    }
    ~TurtlePoseServer() {}
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<TurtlePoseServer>("turtle_pose_server");
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}